#include "Utilities.h"
const double PI = 3.14159265359;
Utilities::Utilities()
{
}


Utilities::~Utilities()
{
}
osg::Matrixd Utilities::headToWorld(osg::Vec3d position, osg::Vec3d angles)
{
	osg::Matrixd rMatrix;
	rMatrix.makeRotate(
		osg::DegreesToRadians(angles.x()), osg::Vec3(0, 1, 0),//heading	
		osg::DegreesToRadians(angles.y()), osg::Vec3(1, 0, 0),//pitch
		osg::DegreesToRadians(angles.z()), osg::Vec3(0, 0, 1));//roll
	double rz, rx, ry;

	osg::Matrixd headPosition;
	headPosition.makeTranslate(position.x(), position.y(), position.z());
	osg::Matrixd head = rMatrix*headPosition;
	return head;
}

osg::Matrixd Utilities::worldToHead(osg::Vec3d position, osg::Vec3d angles)
{

	osg::Matrixd invHead;
	osg::Matrixd head = Utilities::headToWorld(position, angles);
	invHead.invert(head);
	return invHead;
}
osg::Vec2d Utilities::DirectionToHP(osg::Vec3d direction)
{
	direction.normalize();
	osg::Quat quat;
	quat.makeRotate(osg::Vec3d(0, 0, -1), direction);
	osg::Matrixd mat(quat);
	osg::Vec3d hpr(MatrixGetRotation(mat));
	osg::Vec2d hp(hpr.x(), hpr.y());
	return hp;

}
osg::Vec2d Utilities::DirectionToHP(osg::Vec3d direction, osg::Matrixd m)
{
	direction.normalize();
	osg::Quat quat;
	osg::Vec3d up = osg::Vec3d(0, 0, -1) * m;
	quat.makeRotate(up, direction);
	osg::Matrixd mat(quat);
	osg::Vec3d hpr(MatrixGetRotation(mat));
	osg::Vec2d hp(hpr.x(), hpr.y());
	return hp;

}
void	Utilities::MatrixGetRotation(osg::Matrixd mat, double &rz, double &rx, double &ry)
{
	////NO  .RxRyRz	 ,RxRzRy	,RyRxRz		,RyRzRx,
	////double mat[3][3]		 //
	////YES RzRxRy

	//rx = asin(mat(2,1));
	//rz = atan2(-mat(0,1), mat(1, 1));
	//ry = atan2(-mat(2,0), mat(2,2));
	//if(mat(2,1) < +1)
	//{
	//	if( mat(2,1) > -1)
	//	{
	//		rx = asin( mat(2 ,1) );
	//		rz = atan2(-mat(0, 1), mat(1, 1));
	//		ry = atan2(-mat(2, 0), mat(2, 2));

	//	}
	//	else
	//	{
	//		rx = -PI / 2.0;			rx = -PI / 2.0;
	//		rz = atan2(-mat(0, 2), mat(0, 0));

	//		ry = 0;
	//	}
	//}
	//else 
	//{

	//	rx = -PI / 2.0;
	//	rz = atan2(-mat(0, 2), mat(0, 0));
	//	ry = 0;
	//}
	//rx = osg::RadiansToDegrees(-rx);
	//ry = osg::RadiansToDegrees(-ry);
	//rz = osg::RadiansToDegrees(-rz);



	//YES RyRxRz


	if (mat(1, 2) <  1)
	{
		if (mat(1, 2) > -1)
		{
			rx = asin(-mat(1, 2));
			rz = atan2(mat(1, 0), mat(1, 1));
			ry = atan2(mat(0, 2), mat(2, 2));

		}
		else
		{
			rx = PI / 2.0;
			rz = 0;
			ry = -atan2(-mat(0, 1), mat(0, 0));
		}
	}
	else
	{

		rx = -PI / 2.0;
		ry = atan2(-mat(0, 1), mat(0, 0));
		rz = 0;
	}
	rx = osg::RadiansToDegrees(-rx);
	ry = osg::RadiansToDegrees(-ry);
	rz = osg::RadiansToDegrees(-rz);


}
osg::Vec3 	  Utilities::MatrixGetRotation(osg::Matrixd mat)
{
	double tx, ty, tz;
	Utilities::MatrixGetRotation(mat, tz, ty, tx);
	osg::Vec3d res(tx, ty, tz);
	return res;
}
